#include "MyGraspGenerator.h"

//! Function to get grasps of type GraspRepresentationI based on the scene data and scene info 
/*!
 * This is the place holder for your grasp-generation method. The function
 * comes in three different types, depending on the grasp representation
 * that you would like to use. This is for using GraspRepresentationI,
 * that is, based on the contact points.
 *
 * @param sceneData A pointer to the scene data, this contains the 
 * left and right rectified images, the disparity map and the point cloud.
 * @param sceneGrasps A vector where you will have to place all the grasps. 
 * Grasps can be set in three different representations. 
 */
void MyGraspGenerator::getGrasps(SceneData* sceneData, std::vector<GraspRepresentationI*> &sceneGrasps) {
  //! Example of adding a grasp. You can push back more grasps in the sceneGrasps vector
  for(int i=0; i<3; i++) {
    GraspRepresentationI *grasp = new GraspRepresentationI;
    //! Add contact points (2 for parallel grasps, 3 for three-finger grasps)
    grasp->addContactPoint(-300.0, 400.0, 250.0, 0.0, 1.0, 0.0); //<! Position (mm) of contact points and unit vector giving the force direction
    grasp->addContactPoint(-200.0, 400.0, 250.0, 0.0, 1.0, 0.0); //<! Position (mm) of contact points and unit vector giving the force direction
    grasp->addContactPoint(-250.0, 500.0, 250.0, 0.0, -1.0, 0.0);//<! Position (mm) of contact points and unit vector giving the force direction
    grasp->setAOpen(-0.5);
    grasp->setAClose(0.4);
    grasp->setApproachDirection(0.0, 0.0, -1.0);                 //<! Unit vector giving the preferred approach direction of the hand
    sceneGrasps.push_back(grasp);
  }
  std::cout << "  - Generation of grasps for the scene. Type I" << std::endl;
  std::cout << "    + Number of grasp hypotheses: " << sceneGrasps.size() << std::endl;
}

//! Function to get grasps of type GraspRepresentationII based on the scene data and scene info 
/*!
 * This is the place holder for your grasp-generation method. The function
 * comes in three different types, depending on the grasp representation
 * that you would like to use. This is for using GraspRepresentationII,
 * that is, based on hand pre-shapes and hand pose.
 * @param sceneData A pointer to the scene data, this contains the 
 * left and right rectified images, the disparity map and the point cloud.
 * @param sceneGrasps A vector where you will have to place all the grasps. 
 * Grasps can be set in three different representations. 
 */
void MyGraspGenerator::getGrasps(SceneData* sceneData, std::vector<GraspRepresentationII*> &sceneGrasps) {
  //! Example of adding a grasp. You can push back more grasps in the sceneGrasps vector
  for(int i=0; i<3; i++) {
    GraspRepresentationII *grasp = new GraspRepresentationII;
    double handX = -250.0; double handY = 450.0; double handZ = 450.0;   //!< The hand position in mm
    double q0 = 0.0; double q1 = 0.0; double q2 = -1.0; double q3 = 0.0; //!< Quaternion giving the rotation to put the hand a specfic orientation
    grasp->setHandPose(handX, handY, handZ, q0, q1, q2, q3);
    grasp->setPreShapeID(1);                                             //!< pre-shape identifier (0,1, or 2)
    sceneGrasps.push_back(grasp);
  }
  std::cout << "  - Generation of grasps for the scene. Type II" << std::endl;
  std::cout << "    + Number of grasp hypotheses: " << sceneGrasps.size() << std::endl;
}

//! Function to get grasps of type GraspRepresentationIII based on the scene data and scene info 
/*!
 * This is the place holder for your grasp-generation method. The function
 * comes in three different types, depending on the grasp representation
 * that you would like to use. This is for using GraspRepresentationIII,
 * that is, based on joint configuration and hand pose
 * @param sceneData A pointer to the scene data, this contains the 
 * left and right rectified images, the disparity map and the point cloud.
 * @param sceneGrasps A vector where you will have to place all the grasps. 
 * Grasps can be set in three different representations. 
 */
void MyGraspGenerator::getGrasps(SceneData* sceneData, std::vector<GraspRepresentationIII*> &sceneGrasps) {
  //! Example of adding a grasp. You can push back more grasps in the sceneGrasps vector
  for(int i=0; i<3; i++) {
    GraspRepresentationIII *grasp = new GraspRepresentationIII;
    double handX = -250.0; double handY = 450.0; double handZ = 450.0;   //!< The hand position in mm
    double q0 = 0.0; double q1 = 0.0; double q2 = -1.0; double q3 = 0.0; //!< Quaternion giving the rotation to put the hand a specfic orientation
    double j[NR_JOINTS_SDH];                                             //!< The joint configuration (NR_JOINTS_SDH = 7)
    j[0] = -0.18; j[1] = 0.18; j[2] = 0.59; j[3]= 0.0; j[4] = 0.0; j[5] = -0.59; j[6] = 0.59; 

    /*! 
     * Calculate the open and closed joint configurations. Here, we open and close the base joint of all 
     * fingers, but this can be completely defined by the user.
     */
    double aOpen = -0.5;
    double aClose = 0.2;
    double jOpen[NR_JOINTS_SDH];
    double jClose[NR_JOINTS_SDH];
    changeAngleFingerBaseJoints(jOpen, j, aOpen);
    changeAngleFingerBaseJoints(jClose, j, aClose);
    /*!
     * Set the maximum torques for each of the fingers. These are the default values. The user can change
     * these. 
     */
    double maxTau[NR_JOINTS_SDH];
    setDefaultMaxTorques(maxTau);

    // Set the parameters of the grasp
    grasp->setHandPose(handX, handY, handZ, q0, q1, q2, q3);
    grasp->setJointConfigurationOpen(jOpen);
    grasp->setJointConfigurationClose(jClose);
    grasp->setMaxTorques(maxTau);
    sceneGrasps.push_back(grasp);
  }
  std::cout << "  - Generation of grasps for the scene. Type III" << std::endl;
  std::cout << "    + Number of grasp hypotheses: " << sceneGrasps.size() << std::endl;
}

/*!
 * Function to change the angle of the base joints of the three fingers of the
 * Schunk Dexterous Hand.
 * @param jNew The double array that will contain the altered joint configuration
 * @param j The double array with the old joint configuration
 * @param a A double containing the angle that will be added to the base joints of each finger
 */
void MyGraspGenerator::changeAngleFingerBaseJoints(double* jNew, double* j, double a) {
  jNew[0] = j[0] + a; jNew[1] = j[1]; jNew[2] = j[2];
  jNew[3] = j[3] + a; jNew[4] = j[4];
  jNew[5] = j[5] + a; jNew[6] = j[6];
}

/*
 * Function to set the default max torque values for each joint.
 * maxTau Double array that will contain the values after the function has been called.
 */
void MyGraspGenerator::setDefaultMaxTorques(double* maxTau) {
  maxTau[0] = 7;
  maxTau[1] = 2.0;
  maxTau[2] = 2.0;
  maxTau[3] = 10.0;
  maxTau[4] = 2.0;
  maxTau[5] = 2.0;
  maxTau[6] = 2.0;
}

/*!
 * This function creates the collision-info file for this scene. This file consists of
 * a equation of the plane which is assumed to be the table, and a list of 3D points. In
 * order to see if a grasp can be performed, the grasp simulation uses this file to 
 * check if the gripper will penetrate elements in the scene. This is done by detecting
 * collision between the gripper and the table plane or any of the 3D points.
 * The plane is set to the dominant horizontal plane in the point cloud, that is the plane
 * which is more or less horizontal and that has the most supporting points. All points
 * in the point cloud that are above the table plane and withing reach of the robotic arm
 * are added as points to the collision-info file.
 *
 * @param sceneData A pointer to the scene data, this contains the 
 * left and right rectified images, the disparity map and the point cloud.
 * @param sceneInfo A pointer to the scene info.
 */
void MyGraspGenerator::makeCollisionInfoFile(SceneData* sceneData, SceneInfo* sceneInfo, std::string rootPath) {
  using boost::filesystem::path;

  /*!
   * USER_NOTE: This is example code to get the dominant more-or-less horizontal 
   * plane in the scene. The point cloud is furthermore filtered so that the points
   * above the table plane and within reach for the robot are maintained. 
   * The plane equation and the filtered points are stored as collision data.
   * Replace this with your own code.
   */

  /*!
   * PointCloudFilterTable(sigma, r1, r2, horizontalAssumtion)
   *  - sigma. The distance in mm that is allowed for points to support the 
   *    plane. Once the dominant plane is found, all point above sigma distance
   *    from the plane are include in the collision-info file.
   *  - r1. The minimum distance from the base of the robot. Points closer are 
   *    not considered.
   *  - r2. The maximum distance from the base of the robot. Points further are
   *    not considered.
   *  - horizontalAssumtion. Boolean indicating whether a (more-or-less) horizontal
   *    plane is assumed or whether planes of all orientations can be found.
   */
  PointCloudFilterTable filterTable(15.0, 10.0, 800.0, true);
  PointCloud* filteredPointCloud = filterTable.filter(sceneData->pointCloud);

  // Write the collision information to file
  // Check if the directory path already exists and create it if not using boost

  string collisionFile;
  if(rootPath == "")
    collisionFile = sceneInfo->collision;
  else
    collisionFile = rootPath + "/" + sceneInfo->collision;

  path pathCollisionFile(collisionFile);
  boost::filesystem::create_directories(pathCollisionFile.branch_path());

  //FILE *fd = fopen(sceneInfo->collision.c_str(), "w+");

  std::vector<double> plane(4);
  plane[0] = filterTable.getA();
  plane[1] = filterTable.getB();
  plane[2] = filterTable.getC();
  plane[3] = filterTable.getD();

  // On average, this result in something between 10,000 and 20,000 point. 
  // The point cloud is very detailed, which is not necesarry for the 
  // collision detection. We therefore sample each 5th point.
  unsigned int sampleStep = 5;
  std::vector<double*> points;
  for(unsigned int i = 0; i < filteredPointCloud->points.size(); i+=sampleStep){
    points.push_back(filteredPointCloud->points[i]->pos);
  }

  STLWriter stlWriter;
  double widthPointBox = 5; //<! Width of the boxes placed at every point in mm
  stlWriter.writeCollisionFile(collisionFile, plane, points, widthPointBox);

  // Write the plane equation
  //fprintf(fd, "%.10f %.10f %.10f %.10f\n",  filterTable.getA(), filterTable.getB(), filterTable.getC(), filterTable.getD());

  // Write all points to the file
  //for(unsigned int i = 0; i < filteredPointCloud->points.size(); i++){
  //  fprintf(fd, "%f %f %f\n", filteredPointCloud->points[i]->pos[0], filteredPointCloud->points[i]->pos[1], filteredPointCloud->points[i]->pos[2]);
  //}
  //fclose(fd);
  std::cout << "  - Scene information for collision checking: " << std::endl;
  printf("      + Dominant horizontal plane: a=%1.4f, b=%1.4f, c=%1.4f, d=%1.4f\n", 
	 plane[0], plane[1], plane[2], plane[3]);
  std::cout << "      + Nr of selected points above the plane: " << points.size() << std::endl;
  std::cout << "      + Created collision file: " << sceneInfo->collision << std::endl;


  delete filteredPointCloud; 
}

